#include "task_main.h"
#include <stdio.h>
#include <stdarg.h>
#include "Servo.h"
#include "Stepper.hpp"
#include "strutil.hpp"
#include "log.h"
#include "lcd.h"
#include "stm32_adafruit_lcd.h"
#include "zh_iSwitch.h"
#include "Keyboard.h"
#include "arm.hpp"
#include "eGUI.hpp"
#include "eGUI_App.hpp"
#include "eString.hpp"
#include "interface_uart.hpp"
#include "track_config.hpp"

#define LOGTAG "task"
// Servo_t servou;
// Servo_t servod;
extern class Stepper stepper1;
extern class Stepper stepper2;
arm Arm(stepper1, stepper2);

uint32_t Key_Value = 0;
char keyChar = 0;

/* USER CODE BEGIN Header_TaskInit */
/**
 * @brief  Function implementing the InitTask thread.
 * @param  argument: Not used
 * @retval None
 */
/* USER CODE END Header_TaskInit */
void TaskInit(void const *argument)
{
    /* USER CODE BEGIN TaskInit */
    board_base_init(); // DWT, RTT, UART, TIM

    // ServoInit(&servou, &htim3, TIM_CHANNEL_3, 180, 0, 180);
    // ServoInit(&servod, &htim3, TIM_CHANNEL_4, 180, 0, 180);
    colorTrackInit();
    LOG_INFO(LOGTAG, "TaskInit\n");
    vTaskDelete(NULL); // Delete self
    /* USER CODE END TaskInit */
}

/* USER CODE BEGIN Header_TaskInput */
/**
 * @brief  Function implementing the InputTask thread.
 * @param  argument: Not used
 * @retval None
 */
/* USER CODE END Header_TaskInput */
void TaskInput(void const *argument)
{
    /* USER CODE BEGIN TaskInput */
    LOG_INFO(LOGTAG, "TaskInput\n");
    /* Infinite loop */
    for (;;)
    {
        uint32_t Key_Input = (GPIOG->IDR & (SW4_Pin | SW3_Pin | SW2_Pin | SW1_Pin)) >> 12; // 低电平有效
        Key_Input |= Get_Keyborad() << 4;
        Key_Value = iSWx_Handler(Key_Input);

        char key_char = Keyboard_Scan(Key_Value >> 4);

        if(Key_Value>0) Beep(5);
        // if (Key_Value) (LOG_INFO(LOGTAG,"Key_Value=%X\n",Key_Value));
        // if (key_char) (LOG_INFO(LOGTAG,"keychar:%c\n", key_char));
        if (Key_Value & 0x01)
        {
            uint8_t *pS = &iSW_Get_Handle(iSW0)->status;

            if (*pS & 1 << iSW_Mode0_Bit)
            {
				// stepper1.moveStep(1600);
                keyChar = 0x01;
            }
            *pS = 0;
        }

        if (Key_Value & 0x02)
        {
            uint8_t *pS = &iSW_Get_Handle(iSW1)->status;
			// stepper2.moveStep(1600);
            if (*pS & 1 << 0)
                LOG_INFO(LOGTAG, "F2\n\r");
            keyChar = 0x02;
            *pS = 0;
        }
        if (Key_Value & 0x04)
        {
            keyChar = 0x03;
            uint8_t *pS = &iSW_Get_Handle(iSW2)->status;
            // stepper2.moveToStep(0);
            if (*pS & 1 << 0)
                LOG_INFO(LOGTAG, "F3\n\r");
            *pS = 0;
        }
        if (Key_Value & 0x08)
        {
            keyChar = 0x04;
            uint8_t *pS = &iSW_Get_Handle(iSW3)->status;
            // stepper1.moveToStep(0);
            if (*pS & 1 << 0)
                LOG_INFO(LOGTAG, "F4\n\r");
            if (*pS & 1 << iSW_Mode2_Long_Bit)
            {
                Beep(30);
            }
            *pS = 0;
            //		UARTx_Printfs&huart3, tate_on = 1;
        }

        egui.input(keyChar);
        iSW_Clear(iSW_Get_Handle(iSW0), iSW_LONG);
        //	Menu_Input_Num(key_char);
        osDelay(1);
    }
    /* USER CODE END TaskInput */
}

/* USER CODE BEGIN Header_TaskOutput */
/**
 * @brief Function implementing the OutputTask thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_TaskOutput */
void TaskOutput(void const *argument)
{
    /* USER CODE BEGIN TaskOutput */
    LOG_INFO(LOGTAG, "TaskOutput\n");
    // ServoStart(&servou);
    // ServoStart(&servod);
    stepper1.setSpeed(2);
    stepper2.setSpeed(2);
    Arm.configResetOffset(10.0f, 10.0f);
    Arm.configResetSpeed(-2, -2);
    Arm.configResetCnt(1);
    Arm.configGoAimSpeed(2,2);
    /* Infinite loop */
    for (;;)
    {
        Arm.run();
        osDelay(10);
    }
    /* USER CODE END TaskOutput */
}

/* USER CODE BEGIN Header_TaskDisplay */
/**
 * @brief Function implementing the DisplayTask thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_TaskDisplay */
void TaskDisplay(void const *argument)
{
	// int cnt = 0;
    /* USER CODE BEGIN TaskDisplay */
    LOG_INFO(LOGTAG, "TaskDisplay\n");
	BSP_LCD_Init();
    
	BSP_LCD_Clear(LCD_COLOR_RED);
    osDelay(200);
	BSP_LCD_Clear(LCD_COLOR_GREEN);
    osDelay(200);
	BSP_LCD_Clear(LCD_COLOR_BLUE);
    osDelay(200);
	BSP_LCD_Clear(LCD_COLOR_WHITE);
	BSP_LCD_SetFont(&Font24);
	BSP_LCD_SetBackColor(LCD_COLOR_WHITE);
	BSP_LCD_SetTextColor(LCD_COLOR_BLACK);
	BSP_LCD_PrintLine(0, "3.5 TFT TEST");
    BSP_GUI_Init();
    /* Infinite loop */
    for (;;)
    {
        egui.run();
		// BSP_LCD_PrintLine(1, "Number: %d",cnt++);
        // BSP_LCD_PrintLine(2, "KeyValue: %d",Key_Value);
        // BSP_LCD_PrintLine(3, "Steps[%lld,%lld]        ",stepper1.getStep(), stepper2.getStep());
        // BSP_LCD_PrintLine(4, "Angle[%.2f,%.2f]     ",stepper1.getAngle(), stepper2.getAngle());
        // BSP_LCD_PrintLine(5, "L1:%d,L2:%d",stepper1.limitsw1.getState(),stepper2.limitsw1.getState());
        // HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);




        osDelay(50);
    }
    /* USER CODE END TaskDisplay */
}

#if 1
/* USER CODE BEGIN Header_TaskMsgHandle */
/**
 * @brief Function implementing the MsgHandlerTask thread.
 * @param argument: Not used
 * @retval None
 */
/* USER CODE END Header_TaskMsgHandle */
void TaskMsgHandle(void const *argument)
{
    /* USER CODE BEGIN Task_MsgHandle */
    const uint32_t xTicksToWait = portMAX_DELAY; // pdMS_TO_TICKS(0)
    uint32_t ulNotifiedValue;
    BaseType_t xState;
    RX_MSG_t *rxMsg;
    LOG_INFO(LOGTAG, "TaskMsgHandle\n");
    /* Infinite loop */
    for (;;)
    {
        // 获取任务通知值并清空
        xState = xTaskNotifyWait( pdFALSE,
								  0xFFFFFFFF,
								  &ulNotifiedValue,
								  xTicksToWait );
        while (xState == pdPASS)      /* 读到了数据 */
        {
            if (ulNotifiedValue & 0x01) // R1MSG数据
            {
                rxMsg = UART_GetRxMsg(&hrx1);
                if (rxMsg == NULL) break;
                // hrx1.rxCpltFlag = 0;
                Queue_Printn(&htx1, rxMsg->data, rxMsg->size);
                if (strncmp((char*)rxMsg->data, "tv:", 3) == 0)
                    Queue_Printn(&htx3, rxMsg->data + 3, rxMsg->size - 3);
            }
            if (ulNotifiedValue & 0x02) // R2MSG数据
            {
                rxMsg = UART_GetRxMsg(&hrx2);
                if (rxMsg == NULL) break;
                // hrx2.rxCpltFlag = 0;
                // Queue_Printn(&htx2, rxMsg->data, rxMsg->size);
                // UARTx_Printn(hrx2.huart, rxMsg->data, rxMsg->size);
                // SEGGER_RTT_Write(0, (const char*)rxMsg->data, rxMsg->size);
                // SEGGER_RTT_Write(0, "\n", 1);

            }
            if (ulNotifiedValue & 0x04) // R3MSG数据
            {
                rxMsg = UART_GetRxMsg(&hrx3);
                if (rxMsg == NULL) break;
                // hrx3.rxCpltFlag = 0;
                Queue_Printn(&htx1, rxMsg->data, rxMsg->size);
                // UARTx_Printn(hrx3.huart, rxMsg->data, rxMsg->size);
            }
            motor_interface(rxMsg->data, rxMsg->size);
            arm_interface(rxMsg->data, rxMsg->size);
            camera_interface(rxMsg->data, rxMsg->size);
            break;
        }
    }
    /* USER CODE END Task_MsgHandle */
}
#endif
